//
// Created by hoshino on 2021/3/27.
//
/*
 * armor_w 甲板的宽度  cx代码里为 config.armor_W
 * armor_h 甲板的宽度  cx代码里为 config.armor_H
 * mtx  相机内参矩阵 ist
 * OpenCV提供solvePnP函数来求解出已知物体相对于头的三维空间坐标系的旋转和平移向量。
 * 平移向量得到的坐标是以摄像头中心为原点的，要得到正确的转角需要对这个坐标进行平移
 */

#include "../include/3Dpnp.h"
#include <opencv2/opencv.hpp>
#include <vector>
#include <cmath>
#include <glog/logging.h>

#ifdef RADAR_COMP_PNP

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>

cv::Point2d radar_pnp::radar_rotate(cv::Mat &Tvec,double theta_y) const {
    cv::Point2d ans;

    if (!this->status)// 校准失败，使用初始值
    {
        // 初始化欧拉角（rpy）,对应绕z轴，绕y轴，绕x轴的旋转角度
        Eigen::Vector3d euler_angle( -15.0/ 180 * CV_PI, theta_y / 180 * CV_PI, 0);
        // 使用Eigen库将欧拉角转换为旋转矩阵
        Eigen::Matrix3d rotation_matrix1;
        rotation_matrix1 = Eigen::Quaterniond(Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
                                              Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
                                              Eigen::AngleAxisd(euler_angle[0],
                                                                Eigen::Vector3d::UnitX())).toRotationMatrix();
        Eigen::Matrix<double, 3, 1> T_n;
        double theta_x = atan2(rotation_matrix1(2, 1), rotation_matrix1(2, 2));//绕x轴俯仰角
        //绕y轴
        double theta_y = atan2(-rotation_matrix1(2, 0),
                               sqrt(rotation_matrix1(2, 1) * rotation_matrix1(2, 1) +
                                    rotation_matrix1(2, 2) * rotation_matrix1(2, 2)));
        double theta_z = atan2(rotation_matrix1(1, 0), rotation_matrix1(0, 0));//绕z轴
        theta_x = theta_x * (180 / CV_PI);
        theta_y = theta_y * (180 / CV_PI);
        theta_z = theta_z * (180 / CV_PI);
        std::cout << "theta_x: " << theta_x << "theta_y: " << theta_y << "theta_z: " << theta_z << std::endl;
        cv::cv2eigen(Tvec, T_n);
        Eigen::Vector3d P_OC;
        P_OC = -rotation_matrix1.transpose() * T_n;
        std::cout << P_OC << std::endl;
        ans.x = (P_OC(1) + this->radar_coordinate_around.x) / this->arena_w;//宽度
        ans.y = (-P_OC(2) + this->radar_coordinate_around.y) / this->arena_l;//长度
    } else {
        Eigen::Vector3d euler_angle(this->radar_angle.pitch, this->radar_angle.yaw, this->radar_angle.roll);
        // 使用Eigen库将欧拉角转换为旋转矩阵
        Eigen::Matrix3d rotation_matrix1;
        rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
                           Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
                           Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
        Eigen::Matrix<double, 3, 1> T_n;
        cv::cv2eigen(Tvec, T_n);
        Eigen::Vector3d P_OC;
        P_OC = -rotation_matrix1.inverse() * T_n;
        std::cout << P_OC << std::endl;
        ans.x = (P_OC(2, 0) + this->radar_coordinate.x) / this->arena_w;//宽度
        ans.y = (P_OC(1, 0) + this->radar_coordinate.y) / this->arena_l;//长度
    }
    return ans;
}

bool radar_pnp::coordinate_judge(cv::Mat &Tvec, double angle_x, double angle_y, double angle_z) const {
    if (abs(Tvec.at<double>(0) - this->radar_coordinate_around.x) > 200) {
        return false;
    }
    if (abs(Tvec.at<double>(1) - this->radar_coordinate_around.y) > 300) {
        return false;
    }
    if (abs(Tvec.at<double>(2) - this->radar_coordinate_around.z) > 200) {
        return false;
    }
    if (abs(angle_x - this->radar_angle_around.pitch) > 5) {
        return false;
    }
    if (abs(angle_y - this->radar_angle_around.roll) > 5) {
        return false;
    }
    if (abs(angle_z - this->radar_angle_around.yaw) > 5) {
        return false;
    }
    return true;
}

bool radar_pnp::radar_coordinate_init(std::vector<std::vector<cv::Point3f>> &visual_coordinate,
                                      std::vector<std::vector<cv::Point3f>> &visual_coordinate_camara) {
    if (visual_coordinate.size() != visual_coordinate_camara.size()) {
        return false;
    } else if (visual_coordinate.empty() || visual_coordinate_camara.empty()) {
        return false;
    }
    std::vector<Radar_angle> radar_angle_init;
    std::vector<cv::Point3d> radar_coor_init;
    cv::Mat Rvec, Tvec, rotM, invrotM, P;
    for (int i = 0; i < visual_coordinate.size(); ++i) {

        //PnP解算
        cv::solvePnP(visual_coordinate[i], visual_coordinate_camara[i], mtx, dist, Rvec, Tvec, false,
                     cv::SOLVEPNP_ITERATIVE);
        //旋转向量转换为旋转矩阵
        cv::Rodrigues(Rvec, rotM);
        double theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));//绕x轴俯仰角
        //绕y轴
        double theta_y = atan2(-rotM.at<double>(2, 0),
                               sqrt(rotM.at<double>(2, 1) * rotM.at<double>(2, 1) +
                                    rotM.at<double>(2, 2) * rotM.at<double>(2, 2)));
        double theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));//绕z轴
        theta_x = theta_x * (180 / CV_PI);
        theta_y = theta_y * (180 / CV_PI);
        theta_z = theta_z * (180 / CV_PI);
        LOG(INFO) << "theta_x: " << theta_x << "theta_y: " << theta_y << "theta_z: " << theta_z << std::endl;
        printf("theta_x:%.1f  y:%.1f  z:%.1f\n", theta_x, theta_y, theta_z);
        if (this->coordinate_judge(Tvec, theta_x, theta_y, theta_z)) {
            Radar_angle radarAngle;
            radarAngle.pitch = theta_x;
            radarAngle.roll = theta_y;
            radarAngle.yaw = theta_z;
            radar_angle_init.push_back(radarAngle);

            cv::Point3d radar_coor = {Tvec.at<double>(0), Tvec.at<double>(1), Tvec.at<double>(2)};
            radar_coor_init.push_back(radar_coor);
            this->status = true;//初始化成功
        }
    }
    if (this->status) {
        for (int i = 0; i < radar_angle_init.size(); ++i) {
            this->radar_coordinate.x += radar_coor_init[i].x;
            this->radar_coordinate.y += radar_coor_init[i].y;
            this->radar_coordinate.z += radar_coor_init[i].z;

            this->radar_angle.yaw += radar_angle_init[i].yaw;
            this->radar_angle.pitch += radar_angle_init[i].pitch;
            this->radar_angle.roll += radar_angle_init[i].roll;
        }
        this->radar_angle.roll += radar_angle_around.roll;
        this->radar_angle.yaw += this->radar_angle_around.yaw;
        this->radar_angle.pitch += this->radar_angle_around.pitch;
        this->radar_coordinate.x += this->radar_coordinate_around.x;
        this->radar_coordinate.y += this->radar_coordinate_around.y;
        this->radar_coordinate.z += this->radar_coordinate_around.z;

        this->radar_angle.roll = radar_angle_around.roll / (radar_angle_init.size() + 1.0);
        this->radar_angle.yaw = this->radar_angle_around.yaw / (radar_angle_init.size() + 1.0);
        this->radar_angle.pitch = this->radar_angle_around.pitch / (radar_angle_init.size() + 1.0);
        this->radar_coordinate.x = this->radar_coordinate_around.x / (radar_angle_init.size() + 1.0);
        this->radar_coordinate.y = this->radar_coordinate_around.y / (radar_angle_init.size() + 1.0);
        this->radar_coordinate.z = this->radar_coordinate_around.z / (radar_angle_init.size() + 1.0);

    }
    return true;
}

bool radar_pnp::radar_debug_init() {
    double fx = 1084.327641170191;
    double cx = 339.53850341612934;
    double fy = 1084.059984057574;
    double cy = 231.71740007725367;
    this->mtx = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    double k1 = -0.08762162595558533;
    double k2 = 0.3259622746761247;
    double p1 = -0.002323832075315665;
    double p2 = 0.0010020761068527833;
    double k3 = -0.8626108481896815;
    this->dist = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);
    this->radar_angle_around.pitch = 23.826;
    this->radar_angle_around.yaw = 2.32513;
    this->radar_angle_around.roll = -0.3676;
    return true;
};

std::vector<cv::Point2d> radar_pnp::radar_solvePnp(std::vector<std::vector<cv::Point2d>> &aim_coordinate_camara) const {

    static double x = 140 / 2.0 * 1.2;
    static double y = 125 / 2.0 * 1.1;
    //装甲板世界坐标，从右下开始逆时针 4 点。
    static std::vector<cv::Point3f> world_points = {cv::Point3f(-x, y, 0), cv::Point3f(x, y, 0), cv::Point3f(x, -y, 0),
                                                    cv::Point3f(-x, -y, 0)};
    cv::Mat Rvec, Tvec, rotM, invrotM, P;
    cv::Mat tuax, ruax;
    std::vector<cv::Point2d> ans;

    for (auto &i : aim_coordinate_camara) {
        //PnP解算
        cv::solvePnP(world_points, i, this->mtx, this->dist, Rvec, Tvec, false, cv::SOLVEPNP_ITERATIVE);
        //旋转向量转换为旋转矩阵
        cv::Rodrigues(Rvec, rotM);
        //再转换为欧拉角
        Tvec.convertTo(tuax, CV_32F);
        //相机坐标系下装甲板的旋转以及中心点位置信息
        //应该不需要使用旋转矩阵

        //相机坐标系到世界坐标系
        if (this->DEBUG) {
            printf("%f %f %f\n", Tvec.at<double>(0), Tvec.at<double>(1), Tvec.at<double>(2));
            Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> R_n;
            Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> T_n;
            cv::cv2eigen(rotM, R_n);
            cv::cv2eigen(Tvec, T_n);
            double theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));//绕x轴俯仰角
            //绕y轴
            double theta_y = atan2(-rotM.at<double>(2, 0),
                                   sqrt(rotM.at<double>(2, 1) * rotM.at<double>(2, 1) +
                                        rotM.at<double>(2, 2) * rotM.at<double>(2, 2)));
            double theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));//绕z轴
            theta_x = theta_x * (180 / CV_PI);
            theta_y = theta_y * (180 / CV_PI);
            theta_z = theta_z * (180 / CV_PI);
            std::cout << "theta_x: " << theta_x << "theta_y: " << theta_y << "theta_z: " << theta_z << std::endl;
            Eigen::Vector3f P_OC;
            P_OC = -R_n.inverse() * T_n;
            cv::Point2d ans_rotate = radar_rotate(Tvec,theta_y);
        } else {
            cv::Point2d ans_rotate = radar_rotate(Tvec,0);
            cv::Point2d ans_ = {ans_rotate.x / arena_w, ans_rotate.y / arena_l};
            //变换成小地图坐标
            ans.push_back(ans_);
        }

    }


    return ans;
}

#endif // RADAR_COMP_PNP